/*
Infrarood sensoren

Bavo 19/03/11
*/

#include "infrarood.h"

using namespace std;

void IRSensors::execute(void)
{
	while(1)
	{	
		cout << "running" << endl;
		main();
		sleep(1);
	}
}

/** haalt X balletje
 * Geeft de X-coordinaat van het balletje relatief tov de linkersensor, 0 indien geen balletje te zien
 */
int IRSensors::getBx(void)
{
	return bx;
}
/** haalt Y balletje
 * Geeft de Y-coordinaat van het balletje relatief tov de linkersensor, 0 indien geen balletje te zien
 */
int IRSensors::getBy(void)
{
	return by;
}
/** haalt vlag balletje
 * true als er een balletje in zicht is, false indien geen balletje te zien
 */
bool IRSensors::BFlag(void)
{
	return b;
}

int IRSensors::getA1(void)
{
	return a1;
}

int IRSensors::getA2(void)
{
	return a2;
}

int IRSensors::getA3(void)
{
	return a3;
}

int IRSensors::getA4(void)
{
	return a4;
}

bool IRSensors::AFlag(void)
{
	return a;
}

void IRSensors::main (void)
{
	s1=35;	//binnenkomende spanning van I²C
	s2=40;
	s3=0;
	s4=0;
	s5=0;
	s6=15;
	x1= afstand (s1);
	x2= afstand (s2);
	x3= afstand (s3);
	x4= afstand (s4);
	x5= afstand (s5);
	x6= afstand (s6);
		
		if (x3+x4+x5+x6==0){
			a = false;
		}
		else {a=true;}
		

		if (x1+x2==0){
			b = false;
		}
		else {b=true;}


		if (b)
		{
			if(x1 + x2 <= Xnul){
				bx = 0;
				by = 0;
			}
			else{			
				//Xnul is DEFINEd in infrarood.h			
				bx = (Xnul*Xnul + x1*x1 - x2*x2)/(2*Xnul);
				by = sqrt( x1*x1 - bx*bx);
			}
		}
		else 
		{
			bx=0; by=0;
		}
		//printf("IRSensor bx=%d by=%d \n",bx,by);
}

int IRSensors::afstand (int s)
{
	return s*2; //tijdelijke factor, moet nog bepaald worden
}
